

#include "zino.h"
#include <rtthread.h>
#include "rgb.h"
#include <stdlib.h>

#define DBG_TAG "rgb.state"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

uint32_t rgb_state = 0;
rt_uint8_t stateCnt;
rt_uint8_t rgb_scnt = 0;

const uint8_t blue_color[3] = {0, 0, 255};      //
const uint8_t greenColor[3] = {0, 255, 0};      //
const uint8_t yellow_color[3] = {255, 255, 0};  //
const uint8_t red_color[3] = {255, 0, 0};       //
const uint8_t purple_color[3] = {102, 0, 204};  //
const uint8_t white_color[3] = {255, 255, 255}; //

struct rgbColorState
{
    const uint8_t *color;
    uint8_t mode;
};
enum
{
    RGB_STATE_MODE_FAST = 0,
    RGB_STATE_MODE_SLOW,
    RGB_STATE_MODE_BREATH,
};
const char zino_state_str[ZINO_STATE_COUNT][24] = {
    "ACC_GYRO_HARDFAULT",    // 陀螺仪与加速度计错误
    "BARO_HARDFAULT",        // 气压计错误
    "RC_HARDFAULT",          // 无线模块错误
    "OPTICALFLOW_HARDFAULT", // 光流模块错误
    "COMPASS_HARDFAULT",     // 罗盘模块错误
    "IMU_CALIBRATING",       // 水平校准带中
    "RC_PAIRING",            // 无线对频中
    "RC_LOST",               // 无线无信号
    "COMPASS_CALIBRATING",   // 罗盘校准中
    "STANDBY",               // 一切正常，待机...(新手室内)
    "OK_TO_ARM",             // 解锁中
    "EMERGENCY_STOP",        // 急停
    "ARMED",                 // 解锁了
    "ARMING_ALERT",          // 解锁警报
    "STANDARD_INDOOR",       // 标准室内
    "STANDARD_OUTDOOR",      // 标准室外
    "LOW_POWER",             // 低电量
    "LASER_HARDFAULT",
    "PRO_STANDBY",  // 编程一切正常，待机...(新手室内)
    "FACTORY_TEST", // 出厂测试
};
struct rgbColorState rgbColorState[ZINO_STATE_COUNT] =
    {
        {white_color, RGB_STATE_MODE_FAST},    // FC_ACC_GYRO_HARDFAULT
        {white_color, RGB_STATE_MODE_FAST},    // FC_BARO_HARDFAULT
        {blue_color, RGB_STATE_MODE_FAST},     // FC_RC_HARDFAULT
        {white_color, RGB_STATE_MODE_FAST},    // FC_OPTICALFLOW_HARDFAULT
        {white_color, RGB_STATE_MODE_FAST},    // FC_OPTICALFLOW_HARDFAULT
        {red_color, RGB_STATE_MODE_FAST},      // FC_IMU_CALIBRATING
        {blue_color, RGB_STATE_MODE_SLOW},     // FC_RC_PAIRING
        {yellow_color, RGB_STATE_MODE_SLOW},   // FC_RC_LOST
        {red_color, RGB_STATE_MODE_FAST},      // FC_RC_LOST
        {blue_color, RGB_STATE_MODE_BREATH},   // FC_STANDBY
        {greenColor, RGB_STATE_MODE_FAST},     // FC_OK_TO_ARM
        {yellow_color, RGB_STATE_MODE_FAST},   // FC_EMERGENCY_STOP
        {greenColor, RGB_STATE_MODE_BREATH},   // FC_ARMED
        {greenColor, RGB_STATE_MODE_FAST},     // FC_ARMING_ALERK
        {blue_color, RGB_STATE_MODE_BREATH},   // FC_MODE_STANDARD_INDOOR
        {blue_color, RGB_STATE_MODE_BREATH},   // FC_MODE_STANDARD_OUTDOOR
        {red_color, RGB_STATE_MODE_SLOW},      // FC_LOW_POWER
        {white_color, RGB_STATE_MODE_FAST},    // FC_LASER_HARDFAULT
        {purple_color, RGB_STATE_MODE_BREATH}, // FC_PRO_STANDBY
        {white_color, RGB_STATE_MODE_SLOW},    // FC_FACTORY_TEST
                                               // FC_ERROR_COUNT
};

void rgb_state_handle()
{
#define SHOW_TIME 32

    if (rgb_scnt++ >= SHOW_TIME)
    {
        rgb_scnt = 0;
        stateCnt++;
    }
    if (ZINO_STATE_CHECK(stateCnt))
    {
        uint8_t r = 0, g = 0, b = 0;
        // LOG_D("rgb_state:%s %d\n", rgb_state_str[stateCnt], (int)stateCnt);
        switch (rgbColorState[stateCnt].mode)
        {
        case RGB_STATE_MODE_FAST:
            // LOG_D("RGB_STATE_MODE_FAST %d\n", (int)rgb_scnt);
            if ((rgb_scnt % (SHOW_TIME / 4)) < (SHOW_TIME / 8))
            {
                // LOG_D("fast %d\n", (int)rgb_scnt);
                r = rgbColorState[stateCnt].color[0];
                g = rgbColorState[stateCnt].color[1];
                b = rgbColorState[stateCnt].color[2];
            }
            break;
        case RGB_STATE_MODE_SLOW:
            // LOG_D("RGB_STATE_MODE_SLOW %d %d\n", (int)rgb_scnt, (int)stateCnt);
            if (rgb_scnt < (SHOW_TIME / 2))
            {
                r = rgbColorState[stateCnt].color[0];
                g = rgbColorState[stateCnt].color[1];
                b = rgbColorState[stateCnt].color[2];
            }
            break;
        case RGB_STATE_MODE_BREATH:
            // LOG_D("RGB_STATE_MODE_BREATH %d\n", (int)rgb_scnt);
            if (rgb_scnt < (SHOW_TIME / 2))
            {
                r = rgbColorState[stateCnt].color[0] * rgb_scnt / (SHOW_TIME / 2);
                g = rgbColorState[stateCnt].color[1] * rgb_scnt / (SHOW_TIME / 2);
                b = rgbColorState[stateCnt].color[2] * rgb_scnt / (SHOW_TIME / 2);
            }
            else
            {
                r = rgbColorState[stateCnt].color[0] * (SHOW_TIME - rgb_scnt) / (SHOW_TIME / 2);
                g = rgbColorState[stateCnt].color[1] * (SHOW_TIME - rgb_scnt) / (SHOW_TIME / 2);
                b = rgbColorState[stateCnt].color[2] * (SHOW_TIME - rgb_scnt) / (SHOW_TIME / 2);
            }
            // LOG_D("Rgb:%d,%d,%d \n", (int)r, (int)g, (int)b);
            break;
        default:
            break;
        }
        rgb_set(r, g, b);
    }
    else
    {
        while (!ZINO_STATE_CHECK(stateCnt))
        {
            stateCnt++;
            if (stateCnt >= ZINO_STATE_COUNT)
            {
                stateCnt = 0;
                break;
            }
        }
    }
}

static bool error_log_on = 0;
void zinoState(int argc, char *argv[])
{
    // LOG_E("error_log_on=%d",error_log_on);
    if (!rt_strcmp(argv[1], "off"))
    {
        error_log_on = 0;
    }
    else if (!rt_strcmp(argv[1], "on"))
    {
        error_log_on = 1;
    }
    else if (!rt_strcmp(argv[1], "1"))
    {
        error_log_on = 1;
    }
    else if (!rt_strcmp(argv[1], "0"))
    {
        error_log_on = 0;
    }
    else if (!rt_strcmp(argv[1], "set"))
    {
        uint8_t stateCnt = atoi(argv[2]);
        if (stateCnt < ZINO_STATE_COUNT)
        {
            ZINO_STATE_SET(stateCnt);
        }
    }
    else if (!rt_strcmp(argv[1], "clr"))
    {
        uint8_t stateCnt = atoi(argv[2]);
        if (stateCnt < ZINO_STATE_COUNT)
        {
            ZINO_STATE_CLR(stateCnt);
        }
    }
    else
    {
        LOG_I("usage: fc_error [on] [off] [0] [1]");
    }
}
MSH_CMD_EXPORT(zinoState, fc_error on / off);
void zino_state_msg(void)
{
    if (!error_log_on)
        return;

    for (uint8_t i = 0; i < ZINO_STATE_COUNT; i++)
    {
        if (ZINO_STATE_CHECK(i))
        {
            LOG_I("[%02d]:%s\n", i, zino_state_str[i]);
        }
    }
}

void zino_state_task(void *param)
{
    while (1)
    {
        static uint8_t cnt = 0;
        if (cnt++ > 20)
        {
            cnt = 0;
            zino_state_msg();
            // NOTE - 自动关机
            // autoshutdown();
        }
        rgb_state_handle();
        rt_thread_mdelay(50);
    }
}

int zino_task_entry(void)
{

    rt_thread_t thread = rt_thread_create("fc_error_handle", zino_state_task, RT_NULL, 512, 10, 10);
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        LOG_E("zino_task_entry failed.");
        return -RT_ERROR;
    }
    LOG_I("zino state task started.");
    return RT_EOK;
}
ZINO_APP_EXPORT(zino_task_entry);